package com.rockwell.maze800.model;

import net.wimpi.modbus.ModbusException;

public class RobotPositionDetector implements IRobotPositionDetector {
	private Robot m_robot;
	private Maze m_maze;
	
	private IRobotSensors m_sensors;
	private IRobotCommander m_commander;
	
	private boolean m_hasLeftSensor, m_hasRightSensor;
	private RobotPosition m_prevPosition = new RobotPosition(0, false, 0, false, false);
	
	/*
	 * center
			left: 39485
			right: 37466
		
		
		too right
			left: 61813
			right: 16002
		
		too left
			left: 23219
			right: 52564
	 */
	public static double LeftLaneCenterDistance = 41248;
	public static double LeftTooCloseToSideWallDistance = 23219;
	public static double LeftCloseToSideWallDistance = LeftTooCloseToSideWallDistance + (LeftLaneCenterDistance - LeftTooCloseToSideWallDistance)/2;

	public static double RightLaneCenterDistance = 35305;
	public static double RightTooCloseToSideWallDistance = 16002;
	public static double RightCloseToSideWallDistance = RightTooCloseToSideWallDistance + (RightLaneCenterDistance - RightTooCloseToSideWallDistance)/2;
	
	public RobotPositionDetector(Robot robot, Maze maze, IRobotSensors sensors, IRobotCommander commander) {
		m_robot = robot;
		m_maze = maze;
		
		m_sensors = sensors;
		m_commander = commander;
		
		m_hasLeftSensor = sensors.hasLeftDistanceSensor();
		m_hasRightSensor = sensors.hasRightDistanceSensor();
	}
	
	@Override
	public final RobotPosition getPosition() {
		if (m_hasLeftSensor && m_hasRightSensor) {
			return getPositionWithFLRSensors();
		}
		else if (m_hasLeftSensor) {
			return getPositionWithFLSensors();
		}
		else if (m_hasRightSensor) {
			return getPositionWithFRSensors();
		}
		
		// TODO Auto-generated method stub
		return null;
	}

	public final RobotPosition getPreviousPosition()
	{
		return m_prevPosition;
	}
	
	private final RobotPosition getPositionWithFLSensors() {
		return null;
	}

	
	private final RobotPosition getPositionWithFRSensors() {
		return null;
	}

	private final RobotPosition getPositionWithFLRSensors() {
		
		int[] values = m_sensors.getAllSensorStatus();
		
		if (values == null || values.length != 3)
			return null;
		
		int left = values[0];
		int right = values[1];
		int flags = values[2];
		
		boolean front = (flags & IRobotSensors.FRONT_SENSOR_MASK) != 0;
		boolean leftWallPresent = (flags & IRobotSensors.LEFT_SENSOR_MASK) != 0;
		boolean rightWallPresent = (flags & IRobotSensors.RIGHT_SENSOR_MASK) != 0;

		RobotPosition position = new RobotPosition(left, front, right, leftWallPresent, rightWallPresent);
		
		// left and right wall within threshold (detected)
		if (leftWallPresent && rightWallPresent) {
			if (!front)
			{
				if (left <= LeftTooCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.LaneLeft2;
				}
				else if (left <= LeftCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.LaneLeft1;
				}
				else if (right <= RightTooCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.LaneRight2;
				}
				else if (right <= RightCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.LaneRight1;
				}
				else {
					position.location = m_prevPosition.location = RobotLocation.LaneCenter;
				}
			} else {
					position.location = m_prevPosition.location = RobotLocation.DeadEnd;
			}
		}
		else if (leftWallPresent && !rightWallPresent) 
		{
			// right beyond threshold (not detected) and left wall within threshold (detected)
			if (front)
			{
				// left wall within threshold and front wall detected. Opening on right.
				if (left <= LeftTooCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.RightTurnOnlyButTooLeft2;
				}
				else if (left <= LeftCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.RightTurnOnlyButTooLeft1;
				}
				else {
					position.location = m_prevPosition.location = RobotLocation.RightTurnOnly;
				}
			} else 
			{
				// left wall within threshold and front wall not detected. Opening on right.
				if (left <= LeftTooCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.TRightStraightButTooLeft2;
				}
				else if (left <= LeftCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.TRightStraightButTooLeft1;
				}
				else {
					position.location = m_prevPosition.location = RobotLocation.TRightStraight;
				}
			}
		}
		else if (!leftWallPresent && rightWallPresent) 
		{
			// left beyond threshold (not detected) and right wall within threshold (detected)
			if (front)
			{
				if (right <= RightTooCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.LeftTurnOnlyButTooRight2;
				}
				else if (right <= RightCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.LeftTurnOnlyButTooRight1;
				}
				else {
				    position.location = m_prevPosition.location = RobotLocation.LeftTurnOnly;
				}
			} else {
				
				if (right <= RightTooCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.TLeftStraightButTooRight2;
				}
				else if (right <= RightCloseToSideWallDistance) {
					position.location = m_prevPosition.location = RobotLocation.TLeftRStraightButTooRight1;
				}
				else {
			        position.location = m_prevPosition.location = RobotLocation.TLeftStraight;
				}
			}
		}
		else if (!leftWallPresent && !rightWallPresent) 
		{
			// left and right beyond threshold (not detected)
			
			// problem: if we detect opening on left and right straight away we can't detect in which lane are we currently in:
			// too close to left1, too close to left2, center, too close to right1, or too close to right2
			if (front)
			{
				position.location = m_prevPosition.location = RobotLocation.TLeftRight;
			} else {
			    position.location = m_prevPosition.location = RobotLocation.FourWay;
			}
		}
		else
		{
			assert(false);
		}
		
		return position;
	}
	
	public final static boolean isTooLeft(RobotPosition position) {
		
		switch (position.location) 
		{
		case LaneCenter:
		case LaneLeft1:
		case LaneLeft2:
		case RightTurnOnlyButTooLeft2:
		case RightTurnOnlyButTooLeft1:
		case RightTurnOnly:
		case TRightStraightButTooLeft2:
		case TRightStraightButTooLeft1:
		case TRightStraight:
		case DeadEnd:
			if (position.left <= LeftLaneCenterDistance) {
				return true;
			}
		default:
			break;
		}
		
		return false;
	}
	
	public boolean updateFrontWallStatus() throws ModbusException
	{
		return m_sensors.detectFrontWall();
	}
}
